#include<Eigen/Dense>
#include<geometry_msgs/PoseStamped.h>
#include<geometry_msgs/TwistStamped.h>
#include<geometry_msgs/Vector3Stamped.h>
#include<qpOASES.hpp>

using namespace std;


/* #define g 9.8
#define pi 3.14
#define yaw 0
#define m 1.5 */

//框架大小 3*3*1
double x_l=-1.5;
double x_h=1.5;
double y_l=-1.5;
double y_h=1.5;
double z_l=1;
double z_h=2;

int testPos(geometry_msgs::PoseStamped pos)
{
    int flag;
    double x=pos.pose.position.x;
    double y=pos.pose.position.y;
    double z=pos.pose.position.z;
    if (x<=x_h&&x>=x_l
    &&y<=y_h&&y>=y_l
    &&z<=z_h&&z>=z_l)
    {
        flag=3;
    }
    else
    {
        flag=4;
    }
    return flag;

}




USING_NAMESPACE_QPOASES

Eigen::Vector3d optimization(Eigen::Vector3d input,geometry_msgs::PoseStamped pose_,geometry_msgs::TwistStamped twst)
{
    Eigen::Vector3d output;
    real_t sol[3];//解出值

    //K值
    double k1=2;
    double k2=3;
    //飞机位置
    double pos_x=pose_.pose.position.x;
    double pos_y=pose_.pose.position.y;
    double pos_z=pose_.pose.position.z;

    //飞行速度
    double vel_x=twst.twist.linear.x;
    double vel_y=twst.twist.linear.y;
    double vel_z=twst.twist.linear.z;


    //ECBF约束条件

    real_t H[3*3]={2,0,0,
                                0,2,0,
                                0,0,2};
    real_t g_cal[3]={(-2)*input[0],(-2)*input[1],(-2)*input[2]};
    real_t A[6*3]=
        {1,0,0,
        -1,0,0,
        0,1,0,
        0,-1,0,
        0,0,1,
        0,0,-1};
    real_t ubA[6*1]={k1*(x_h-pos_x)+k2*(-vel_x),
                                        k1*(pos_x-x_l)+k2*vel_x,
                                        k1*(y_h-pos_y)+k2*(-vel_y),
                                        k1*(pos_y-y_l)+k2*vel_y,
                                        k1*(z_h-pos_z)+k2*(-vel_z),
                                        k1*(pos_z-z_l)+k2*vel_z};
    QProblem flight(3,6);
    //Options options;
	//flight.setOptions( options );

    int_t nWSR=10;
    flight.init( H,g_cal,A,nullptr,nullptr,nullptr,ubA, nWSR);
    
    flight.getPrimalSolution(sol);

    for(int i=0;i<3;i++)
    {
        output[i]=sol[i];
    }
    //ROS_INFO("output_2=%f,%f,%f",sol[0],sol[1],sol[2]);
    return output;
}
